%% 该代码作用为根据已有量测和代码前面的参数设置实现基于EKF的DSE

clear;
clc;

%% Power Flow calculation
%% Case 39 bus data 
% *****************************************
mpc = realcase9;
Y=Ybus_new(mpc);                 % 计算节点导纳矩阵
result=runpf(mpc);               % 求解该系统潮流
f0=60; 
w_syn=2*pi*f0; % 同步转速

Xd=[0.06080; 0.11980; 0.18130];
R=[0;0;0];
H=[23.64; 6.4; 3.010];
D=[0.0125;0.0034;0.0016];
% Xd=[0.006; 0.0697; 0.0531; 0.0436; 0.132; 0.05; 0.049; 0.057; 0.057; 0.031]; % 理想模型中的发电机参数
% H=[500; 30.3; 35.8; 28.6; 26; 34.8; 26.4; 24.3; 34.5; 42];      % 理想模型中的发电机惯性常数
% R=zeros(length(Xd), 1); 
% D=[0; 0;0 ;0; 0; 0; 0; 0; 0; 0];  % 阻尼系数
% *****************************************

D=D/w_syn; % 阻尼系数/同步转速
M=2*H/w_syn; % 文章中表达方法是w0/2H

Vmag=result.bus(:, 8);              % Pu voltage magnitude of each buses 
Vph=result.bus(:, 9);               % angle in degree
V=Vmag.*exp(1j*Vph*pi/180);         % 系统电压
P_jQ=conj(V).*(Y*V);                % Net Power at each node 网络各节点潮流
S=conj(P_jQ);                       % 网络节点潮流
S=S/mpc.baseMVA;                            % 标幺
Sg=result.gen(:, 2)+1j*result.gen(:, 3);    % 发电机输出功率
Sg=Sg/mpc.baseMVA;                          % 标幺
gen_bus=result.gen(:, 1);  

%% 下面进入暂态分析
%% calculate Y22
Y22=diag(1./(1j*Xd));                   % 电机阻抗理想参数

%% Calculation of Y11           % 理想参数线路自导纳
SL=result.bus(:, 3)+1j*result.bus(:, 4);    % 计算结果中节点功率
SL=SL/mpc.baseMVA;                                  % 标幺
YL=conj(SL)./(abs(V).^2);                   % 求节点负荷等效导纳
Y11=Y+diag(YL);                             % 导纳矩阵加上负荷
Y11(gen_bus, gen_bus)=Y11(gen_bus, gen_bus)+Y22;    % 在自导纳上加上发电机的导纳

%% Calculation of Y12 and Y21
% Calculation of Y12
Y12=zeros(length(result.bus(:,1)), length(result.gen(:,1)));    % 节点数×发电机数
%Y12(gen_bus, gen_bus)=Y12(gen_bus, gen_bus)-Y22; 

for i=1:length(result.bus(:,1))                 % 遍历bus
    for k=1:length(result.gen(:,1))             % 遍历gen
        q=result.gen(k,1);                      % 储存q为gen所在的节点编号
        if i==q                                 % 对所有gen在的节点
            Y12(q,k)=-1/(R(k)+Xd(k)*1j);        % 计算Y12为发电机的导纳
        end
    end
end

Y21=transpose(Y12);                             % 转置
%% Calculation of reduced matrix before fault 理想系统故障前的简化矩阵
Ybf=Y22-Y21*inv(Y11)*Y12 ;                      % Ybf=Y22-Y21*Y11逆*Y12

% Bus Reconstruction matrix 节点重构矩阵
RV(:, :, 1)=-inv(Y11)*Y12;

%% Enter fault here to calculate the afterfault and during fault reduced 故障点位置（应该是认为运行过程中节点4接地，切除节点4到节点14的线路后故障消除）
%% ！！！！！！！！！这里注意，一般运行状态下不会马上得知故障点、故障类型，于是需要一点诊断或预测步骤得到故障点故障类型！！！！！！！！！！！
% matrices
f11=5;      % 运行过程中节点4接地
F=[5 6];   % 故障结束后节点4和节点14间的线路断开
f1=F(1);
f2=F(2);
%% during fault 计算故障中的导纳矩阵
Y11df=Y11;                                      % 导纳矩阵加负荷
Y11df(f11, :)=[];                               % 对应行去掉
Y11df(:,f11)=[];                                % 对应列去掉
Y12df=Y12;                                      % Y12计算
Y12df(f11, :)=[];                               % 对应行去掉
Y21df=transpose(Y12df);                         % 取转置
% during fault reduced matrics
Ydf=Y22-Y21df*inv(Y11df)*Y12df;                 % 重新计算Ydf

RV(:, :, 2)=zeros(size(RV(:, :, 1)));
RV(1:end-1, :, 2)=RV(1:end-1, :, 2)-inv(Y11df)*Y12df;


%% afterfault Y11                               % 故障后的Y11，此时线路切除
 Y11after=Y11;                                  % 故障后的Y11中的元素置零
 Y11after(f1,f2)=0;
 Y11after(f2,f1)=0;
for i=1:length(result.branch(:,1))  % 遍历所有支路
    if (f1==result.branch(i,1)&& f2==result.branch(i,2))||(f2==result.branch(i,1)&& f1==result.branch(i,2))     % 支路的始末端为故障的始末端
        Y11after(f1,f1)=Y11after(f1,f1)-result.branch(i,5)*1j/2-1/(result.branch(i,3)+result.branch(i,4)*1j);   
        Y11after(f2,f2)=Y11after(f2,f2)-result.branch(i,5)*1j/2-1/(result.branch(i,3)+result.branch(i,4)*1j);   % 导纳矩阵重新计算（支路断开的计算方法）
    end
end

% Afterfault reduced matrix is 
Yaf=Y22-Y21*inv(Y11after)*Y12 ;                 % 故障后
%RV_af=-inv(Y11after)*Y12 ; 

RV(:, :, 3)=-inv(Y11after)*Y12; 
%% Initialization 初始化系统
deltt=0.0005;   % 步长2000Hz的仿真频率
t_SW=1;         % 故障开始时刻
t_FC=1.0333;    % 故障结束时刻
t_max=10;       % 仿真结束时间
% t_response = 0.02;
% t_SW = t_SW + t_response;
% t_FC = t_FC + t_response;
Ig=conj(Sg./V(1:length(result.gen(:, 1)))); % 发电机电流
% Ig=conj(Sg./V(result.gen(:, 1))); % 发电机电流
E0=V(gen_bus)+Ig.*(R+1j*Xd);                % 等效理想电源机端电压
E_abs=abs(E0);

I0=Ybf*E0;                                  % 理想电源输出电流
delta0=angle(E0)*180/pi;                    % 没有用到  % 发电机初始电压相角  
w0=zeros(length(Xd), 1);                    % w0都设为0
X_0=[angle(E0); w0];                        % X_0为初始电压相角和w0的集合，w0为转速，初值设为0

% Initialize power injection
PG0=real(E0.*conj(I0));         % 理想发电机有功输入
PM=PG0;                         % 代替了PG0不知道为什么要多这一步，PG0后面都没用到
QG0=imag(E0.*conj(I0));         % 理想发电机无功输入

YBUS(:, :, 1)=Ybf;              % 定义YBUS
YBUS(:, :, 2)=Ydf; 
YBUS(:, :, 3)=Yaf;              % 给三维数组分别赋故障前中后三个时间的导纳矩阵
n=length(gen_bus);              % n个发电机节点
s=length(result.bus(:, 1));     % s个节点


%% Estimated State: 对系统的状态估计
% Number of states and measurements 状态量和测量量的个数
ns=2*n;             % 2n个状态量
nm=2*n+2*s;         % 2n+2s个测量量 n个发电机的有功无功 n个节点的电压幅值相角



% Covariance Matrix
sig=1e-2;         % 量测方差
P=sig^2*eye(ns);  % Error covariance matrix 协方差矩阵
Q=sig^2*0.1*eye(ns);  % system noise covariance matrix 系统模型误差
R=sig^2*0.1*eye(nm);  % measurment noise covariance matrix 测量噪声方差
load Z_measure.mat  % 量测值
load X_true.mat     % 状态值

X_hat=X_0;  % 估计值初始化，相角为E0（）
X_est=[];   % 估计值
X_mes=[];   % Initial statel 
Z_est=[];
zhat=[];
% constant values 

RMSE=[]; % 对比标准
cnt = 0;    % 用于计数实现按不同频率调用EKF算法求解状态
T = 10; % 每T个时间间隔调用EKF，该参数也用于后续绘图步骤
% Extended Kalman Filter (EKF) ALgorithm 使用扩展卡尔曼滤波算法求解
for k=0:deltt:t_max     % 仿真出从0到结束的模型
    cnt = cnt+1;
    % Ybus and reconstruction matrix accodring to the requirement 给ps赋值以定下该时刻系统的运行状态
    if k<t_SW
        ps=1;
    elseif (t_SW<k)&&(k<=t_FC)
        ps=2;
    else 
        ps=3;
    end  

    Ybusm = YBUS(:,:,ps);           % 取出对应状态下的Ybus
    RVm=RV(:, :, ps);               % 对应状态下的RV

    [~, X] = ode45(@(t,x) dynamic_system(t,x,M,D,Ybusm,E_abs,PM,n),[k k+deltt],X_0);        % 用ode45求解器求解理想系统动态过程
    X_0=transpose(X(end, :));       % 对X最后一行做转置，得到一列，X是上面求解器得到的结果，为理想状态下的状态值    
    if mod(cnt,T)==0              % 每500ms一次的计算
        z = Z_measure(:,cnt);       % 量测量对应的理想系统真值，格式：[PG; QG; Vmag; Vangle]

        % determine Phi=df/fx 
        Phi=RK4partial(E_abs, X_hat, Ybusm, M, deltt*T, D, n);        % Phi是状态转移矩阵，里面出现偏导数是因为非线性问题线性化
        X_hat=RK4(n, deltt*T, E_abs, ns, X_hat, PM, M, D, Ybusm);     % 算X的估计值
        P=Phi*P*transpose(Phi)+Q;

        % correction 
        [H, zhat]=RK4H(E_abs, X_hat, Ybusm, s,n, RVm,zhat) ;     % 得到偏导数H和由xhat得到的量测计算值

        % Measurement update of state estimate and estimation error covariance 
        K=P*transpose(H)*(H*P*transpose(H)+R);  % 卡尔曼增益
        X_hat=X_hat+ K*(z-zhat);                 % X的最优估计值，这里的z我认为有问题，应该加入一些量测噪声
        P=(eye(ns)-K*H)*P;                      % 认为预测步协方差矩阵为I，更新状态估计协方差矩阵

        X_est=[X_est, X_hat];
        Z_est=[Z_est, zhat];
        RMSE=[RMSE, sqrt(trace(P))];
    end
end

save('39_RMSE_EKF.mat', 'RMSE')

%% Plots
t = (0:deltt:t_max);
for i=1:1:n
    figure(i)
    subplot(3,1,1)
    plot(t,X_true(i, :), 'linewidth', 1.5)
    hold on              % 每500ms一次的计算
    plot(t(1:T:end-1), X_est(i, :), 'color', 'r', 'linewidth', 1);
    % plot(t, X_est(i, :), 'linestyle', '--', 'color', 'r', 'linewidth', 2);
    grid on
    ylabel(sprintf('功角_{%d}', i), 'fontsize', 12)
    xlabel('时间(s)', 'fontsize', 15); 
    title('功角真实值与估计值 \delta', 'fontsize', 12)
    legend(sprintf('功角_{%d, 真实} ',i), sprintf('功角_{%d, EKF估计}', i)); 

    subplot(3,1,2)
    plot(t,X_true(i+n, :), 'linewidth', 1.5)
    hold on     
    plot(t(1:T:end-1), X_est(i+n, :),  'color', 'r', 'linewidth', 1);
    % plot(t, X_est(i+n, :), 'linestyle', '--', 'color', 'r', 'linewidth', 2);
    grid on
    ylabel(sprintf('转速_{%d}', i), 'fontsize', 12)
    xlabel('时间(s)', 'fontsize', 15); 
    title('转速真实值与估计值 \omega', 'fontsize', 12)
    legend(sprintf('转速_{%d, 真实} ',i), sprintf('转速_{%d, EKF估计}', i));

    % --------------------test--------------------------
    % if mode(t,10)==0              % 每500ms一次的计算
    %     plot(t*10,X_true(i+n, 0:10:t_max/deltt)-X_est(i+n, :))
    % end
    subplot(3,1,3)
    plot(t(1:T:end-1),X_true(i+n, (1:T:end-1))-X_est(i+n,:), 'linewidth', 1)
    ylabel(sprintf('转速估计误差_{%d}',i));
    xlabel('时间(s)')
    title(sprintf('转速真实值与估计值的差值 \Delta \omega'))
    % plot(t,X_true(i+n, :)-X_est(i+n,:), 'linewidth', 1.5)
    
    % --------------------test--------------------------

    % subplot(2,2,3)
    % plot(t,X_mes(i+1, :), 'linewidth', 1.5)
    % hold on 
    % plot(t, X_est(i+1, :), 'linestyle', '--', 'color', 'r', 'linewidth', 2);
    % grid on
    % ylabel(sprintf('Angle_{%d}', i+1), 'fontsize', 12)
    % xlabel('time(s)', 'fontsize', 15); 
    % title('Measured Vs Eistimated \delta', 'fontsize', 12)
    % legend(sprintf('Angle_{%d, Actual} ',i+1), sprintf('Angle_{%d, EKF}', i+1), 'fontsize', 10, 'Location', 'northwest'); 
    % 
    % subplot(2,2,4)
    % plot(t,X_mes(i+n+1, :), 'linewidth', 1.5)
    % hold on 
    % plot(t, X_est(i+n+1, :), 'linestyle', '--', 'color', 'r', 'linewidth', 2);
    % grid on
    % ylabel(sprintf('Speed_{%d}', i+1), 'fontsize', 12)
    % xlabel('time(s)', 'fontsize', 15); 
    % title('Measured Vs Eistimated \omega', 'fontsize', 12)
    % legend(sprintf('Speed_{%d, Actual} ',i+1), sprintf('Speed_{%d, EKF}', i+1), 'fontsize', 10, 'Location', 'northwest');

end

% % results with respect to the center of inertia
% MT = sum(M);
% for k = 1:length(X_est(1, :))
%     d_oe = sum(X_est(1:n, k).*M')/MT;
%     d_oa = sum(X_mes(1:n, k).*M')/MT;
    
%     w_oe = sum(X_est(n+1:2*n, k).*M')/MT;
%     w_oa = sum(X_mes(n+1:2*n, k).*M')/MT;
    
%     Xcoie(k, :) = X_est(:, k) - [d_oe, w_oe]';
%     Xcoia(k, :) = X_mes(:, k) - [d_oa, w_oa]';

% end

% %% Plots
% t= (0:deltt:t_max);
% for i=1:n
% figure(i+n)
% subplot(2,1,1)
% plot(t,Xcoia(:, i), 'linewidth', 1.5)
% hold on 
% plot(t, Xcoie(:, i), 'linestyle', '--', 'color', 'r', 'linewidth', 2);
% grid on
% ylabel(sprintf('Angle_{%d}', i), 'fontsize', 12)
% xlabel('time(s)', 'fontsize', 15); 
% title('Measured Vs Eistimated \delta with respect to COI for UKF', 'fontsize', 12)
% legend(sprintf('Angle_{%d, Actual} ',i), sprintf('Angle_{%d, UKF}', i), 'fontsize', 12); 

% subplot(2,1,2)
% plot(t,Xcoia(:, i+n), 'linewidth', 1.5)
% hold on 
% plot(t, Xcoie(:, i+n), 'linestyle', '--', 'color', 'r', 'linewidth', 2);
% grid on
% ylabel(sprintf('Speed_{%d}', i), 'fontsize', 12)
% xlabel('time(s)', 'fontsize', 15); 
% title('Measured Vs Eistimated \omega with respect to COI for UKF', 'fontsize', 12)
% legend(sprintf('Speed_{%d, Actual} ',i), sprintf('Speed_{%d, UKF}', i), 'fontsize', 12); 
% end



% figure(i+n)
% subplot(2,1,1)
% plot(t,X_mes(3, :)-X_mes(1, :), 'linewidth', 1.5)
% hold on 
% plot(t, X_est(3, :)-X_est(1, :), 'linestyle', '--', 'color', 'r', 'linewidth', 2);
% grid on
% ylabel(sprintf('Angle_{%d}', i), 'fontsize', 12)
% xlabel('time(s)', 'fontsize', 15); 
% title('Measured Vs Eistimated \delta with UKF', 'fontsize', 12)
% legend(sprintf('Angle_{%d, Actual} ',i), sprintf('Angle_{%d, UKF}', i), 'fontsize', 12); 

% subplot(2,1,2)
% plot(t,X_mes(2, :)-X_mes(1, :), 'linewidth', 1.5)
% hold on 
% plot(t, X_est(2, :)-X_est(1, :), 'linestyle', '--', 'color', 'r', 'linewidth', 2);
% grid on
% ylabel(sprintf('Angle_{%d}', i), 'fontsize', 12)
% xlabel('time(s)', 'fontsize', 15); 
% title('Measured Vs Eistimated \delta with UKF', 'fontsize', 12)
% legend(sprintf('Angle_{%d, Actual} ',i), sprintf('Angle_{%d, UKF}', i), 'fontsize', 12);

%% func
function Phi=RK4partial(E_abs, X_hat, Ybusm, M, deltt, D, n)
    
    % determine k1 for delta and omega of the dynamic equation dynamics 求解动态方程中的delta和omega需要使用的k1
    E1=E_abs.*exp(1j*X_hat(1:n));     
    I1=Ybusm*E1; 
    dE1=[diag(1j*E_abs.*exp(1j*X_hat(1:n))), zeros(n)];
    dI1=Ybusm*dE1; 
    dPG1=real(conj(diag(I1))*dE1+diag(E1)*conj(dI1)); 
    %k1_w
    d_k1w=(-1*deltt*diag(M.^-1))*dPG1+[zeros(n), (-1*deltt)*diag(D).*diag(M.^-1)];    
    d_w=[eye(n), zeros(n)]+d_k1w;
    
    %k1delta
    k1_delta=[zeros(n), deltt*eye(n)];
    d_delta=[zeros(n), eye(n)]+k1_delta; 
    Phi=[ d_delta; d_w];

end 

function [H, zhat]=RK4H(E_abs, X_hat, Ybusm, s,n, RVm,zhat) 
    % calculate zhat
    E1=E_abs.*exp(1j*X_hat(1:n));     
    I1=Ybusm*E1; 
    PG1=real(conj(I1).*E1); 
    QG1=imag(conj(I1).*E1);
    Vmag=abs(RVm*E1); 
    Vangle=angle(RVm*E1); 
    if ~isempty(zhat)
        Vangle = fix((zhat(2*n+s+1:2*n+2*s)-Vangle+pi)/(2*pi))*2*pi+Vangle;
    end
    zhat=[PG1; QG1; Vmag; Vangle]; 
    
    % calculate H
    dE1=[diag(1j*E_abs.*exp(1j*X_hat(1:n))), zeros(n)];
    dI1=Ybusm*dE1;    
    dPG1=real(conj(diag(I1))*dE1+diag(E1)*conj(dI1));
    dQG1=imag(conj(diag(I1))*dE1+diag(E1)*conj(dI1));
    dVmag=zeros(s, 2*n); 
    dVangle=[ones(s, n), zeros(s, n)];
    H=[dPG1; dQG1; dVmag; dVangle]; 
end 


%  USE ode45 to solve this differential equation 
function dx = dynamic_system(~,x,M,D, Ybusm,Vo,Pm,NumG)  % dynamics of the sin control system
Vg = Vo.*exp(1j*x(1:NumG));
Ibus = Ybusm*Vg;
S = conj(Ibus).*Vg;
Pe = real(S);
dx = zeros(2*NumG,1);
dx(1:NumG) = x(NumG+1:2*NumG);
dx(NumG+1:2*NumG) = (Pm-Pe)./M-D.*x(NumG+1:2*NumG)./M;
end 


function xbreve=RK4(n, deltt, E_abs, ns, X_hat, PM, M, D, Ybusm)
    % update sigma point 
    E1=E_abs.*exp(1j*X_hat(1:n, :)); 
    I1=Ybusm*E1; 
    PG1=real(E1.*conj(I1)); 
    % PM_rep=repmat(PM, 1, 2*ns);
    % M_rep=repmat(M, 1, 2*ns);
    % D_rep=repmat(D, 1, 2*ns); 
    
    % k1
    k1_w=deltt*(M.^-1.*(PM-PG1-(D.*(X_hat(n+1:ns, :)))));
    k1_delta=deltt*X_hat(n+1:ns, :);
    
    % k2
    E2=E_abs.*exp(1j*(X_hat(1:n, :)+k1_delta/2)); 
    I2=Ybusm*E2; 
    PG2=real(E2.*conj(I2));     
    k2_w=deltt*(M.^-1.*(PM-PG2-(D.*((X_hat(n+1:ns, :)+k1_w/2)))));
    k2_delta=deltt*(X_hat(n+1:ns, :)+k1_w/2); 
    
    % k3
    E3=E_abs.*exp(1j*(X_hat(1:n, :)+k2_delta/2)); 
    I3=Ybusm*E3; 
    PG3=real(E3.*conj(I3));     
    k3_w=deltt*(M.^-1.*(PM-PG3-(D.*((X_hat(n+1:ns, :)+k2_w/2)))));
    k3_delta=deltt*(X_hat(n+1:ns, :)+k2_w/2); 
    
    % k2
    E4=E_abs.*exp(1j*(X_hat(1:n, :)+k3_delta)); 
    I4=Ybusm*E4; 
    PG4=real(E4.*conj(I4));     
    k4_w=deltt*(M.^-1.*(PM-PG4-(D.*((X_hat(n+1:ns, :)+k3_w)))));
    k4_delta=deltt*(X_hat(n+1:ns, :)+k3_w); 
    
    xbreve(1:n, :)=X_hat(1:n, :)+ (k1_delta+2*k2_delta+2*k3_delta+k4_delta)/6;
    xbreve(n+1:ns, :)=X_hat(n+1:ns, :) + (k1_w+2*k2_w+2*k3_w+k4_w)/6;
end


